/*
 * Entech2013DriveSubsystem.cpp
 *
 *  Created on: Feb 3, 2013
 *      Author: N500432
 */

#include "ClimberSubsystem.h"

#include "Entech2013Robot.h"
#include "TilterSubsystem.h"
#include "EntechPorts.h"

#include <math.h>

// Default constructor
ClimberSubsystem::ClimberSubsystem() :
	EntechSubsystem("ClimberSubsystem")
	, m_state(kPositioningChains)
	, m_autoDeadman(false)
    , m_leftManual(0)
    , m_rightManual(0)
	, m_leftAtStart(false)
	, m_rightAtStart(false)
	, m_leftHooked(false)
	, m_rightHooked(false)
	, m_leftNoloadCurrent(0.0)
	, m_rightNoloadCurrent(0.0)
	, m_motorSpeedDelta(0.0)
{
    m_leftMotor  = new CANJaguar(c_ClimberLeftMotorCANid);
    m_rightMotor = new CANJaguar(c_ClimberRightMotorCANid);
    m_leftPositionSwitch = new DigitalInput(c_leftClimberPositionSwitch);
    m_rightPositionSwitch = new DigitalInput(c_rightClimberPositionSwitch);
}

// Default destructor
ClimberSubsystem::~ClimberSubsystem()
{
}

void ClimberSubsystem::DoRobotInit(void)
{
	m_leftMotor->ConfigEncoderCodesPerRev(250);
	m_leftMotor->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
	m_rightMotor->ConfigEncoderCodesPerRev(250);
	m_rightMotor->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
}

void ClimberSubsystem::DoDisabledInit(void)
{
}

void ClimberSubsystem::DoDisabled(void)
{
    Stop();
}

void ClimberSubsystem::DoTeleopInit(void)
{
}

void ClimberSubsystem::TeleopInputs(bool deadman, bool leftForw, bool leftBack, bool rightForw, bool rightBack)
{
	m_autoDeadman = deadman;
	m_leftManual = 0;
	if (leftBack) {
		m_leftManual = -1;
	} else if (leftForw) {
		m_leftManual = 1;
	}
	m_rightManual = 0;
	if (rightBack) {
		m_rightManual = -1;
	} else if (rightForw) {
		m_rightManual = 1;
	}
}

void ClimberSubsystem::DoTeleop(void)
{
	float left_pos, right_pos;
	float left_I, right_I;
	double left_sp, right_sp, climb_sp;
	
	climb_sp = c_climb_speed;
	left_pos  = m_leftMotor->GetPosition();
	left_I    = m_leftMotor->GetOutputCurrent();
	right_pos = m_rightMotor->GetPosition();
	right_I   = m_rightMotor->GetOutputCurrent();
	
	// Do Climb Operations
	if (m_autoDeadman) {
		switch (m_state) {
		case kPositioningChains:
			left_sp = right_sp = climb_sp;
			if (!m_leftAtStart) {
				m_leftNoloadCurrent = max(m_leftNoloadCurrent, left_I);
				if (m_leftPositionSwitch->Get()) {
					left_sp = 0.0;
					m_leftAtStart = true;
				}
			}
			if (!m_rightAtStart) {
				m_rightNoloadCurrent = max(m_rightNoloadCurrent, right_I);
				if (m_rightPositionSwitch->Get()) {
					right_sp = 0.0;
					m_rightAtStart = true;
				}
			}
			if (m_leftAtStart && m_rightAtStart) {
				m_state = kHooking1;
			    m_leftEncStart = left_pos;
				m_rightEncStart = right_pos;
				Stop();
			} else {
				SetMotors(left_sp, right_sp);
			}
			break;
		case kHooking1:
			left_sp = right_sp = climb_sp;
			if (!m_leftHooked) {
				if (left_I > 1.2*m_leftNoloadCurrent) {
					left_sp = 0.0;
					m_leftHooked = true;
				}
			}
			if (!m_rightHooked) {
				if (right_I > 1.2*m_rightNoloadCurrent) {
					right_sp = 0.0;
					m_rightHooked = true;
				}
			}
			if (m_leftHooked && m_rightHooked) {
				m_state = kClimbing;
				Stop();
			} else {
				SetMotors(left_sp, right_sp);
			}
			break;
		case kClimbing:
			left_sp = right_sp = climb_sp;
			// Subtract off the found starting positions
			left_pos -= m_leftEncStart;
			right_pos -= m_rightEncStart;
			if ( left_pos > right_pos ) {
				m_motorSpeedDelta += 0.005;
			} else if ( left_pos < right_pos ) {
				m_motorSpeedDelta -= 0.005;
			}
			if ((left_pos  > c_encoderTilterRetract) &&
				(right_pos > c_encoderTilterRetract)    ) {
				ENTECH_ROBOT.GetTilter()->SetPosition(TilterSubsystem::kFlat);
			}
			if ((left_pos  > c_encoderStopClimb) &&
				(right_pos > c_encoderStopClimb)    ) {
				Stop();
				m_state = kDumping;
			} else {
				SetMotors(left_sp - m_motorSpeedDelta, right_sp + m_motorSpeedDelta);
			}
			break;
		case kDumping:
		case kRetract:
			break;
		case kDone:
			Stop();
			break;
		}
	} else {
		// In manual override mode, move motors based on user input
		left_sp  = 0.0;
		right_sp = 0.0;
		if (m_leftManual > 0) {
			left_sp =  c_manual_climb_speed;
		} else if (m_leftManual < 0) {
			left_sp = -c_manual_climb_speed;
		}
		if (m_rightManual > 0) {
			right_sp =  c_manual_climb_speed;
		} else if (m_rightManual < 0) {
			right_sp = -c_manual_climb_speed;
		}
		SetMotors(left_sp, right_sp);
	}
	
	// Print State Information
    DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
    dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Encoders: Left %f  - Right %f",
    		left_pos, right_pos);
    dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Climb I: %f/%f  | %f/%f",
    		left_I, m_leftNoloadCurrent, right_I, m_rightNoloadCurrent);
}

// Manage motor inversions and set motor speeds
void ClimberSubsystem::SetMotors(double lmotor, double rmotor)
{
	lmotor = min(1.0,max(-1.0,lmotor));
	rmotor = min(1.0,max(-1.0,rmotor));
    m_leftMotor->Set(c_left_inverter*lmotor);
    m_rightMotor->Set(c_right_inverter*rmotor);
}

void ClimberSubsystem::Stop(void)
{
    m_leftMotor->StopMotor();	
    m_rightMotor->StopMotor();
}
